#include "../common/common_motor.h"
#include "../common/device_manager.h"
#include "../../bsp/bsp.h"
#include "hcfa.h"
#include "lifter.h"
#include <stdlib.h>
#include "aos/kernel.h"
#include "aos/kv.h"
#include "k_api.h"

#define LIFTER_MOTOR_NUMBER   (1)

#if (LIFTER_MOTOR_NUMBER <= 0)
#error "Invaild number of lifter motor."
#endif

struct xp_lifter {
    u8 inited;          //init state
    u8 check_zero;      //check zero state
    u8 run;             //lifter run state
    u8 error;
    u8 motor_number;    // 电机数量
    struct {
        int origin;
        int current;
        int next;
    } angle_offset; // 折算为电机旋转角度信息
    struct {
        float max;
        float min;
        float current;
        float next;
    } height_offset;// 升降高度信息
    float move_speed;   //lifter line speed m/s
    int warn_code[LIFTER_MOTOR_NUMBER];
    u8 speed_block[LIFTER_MOTOR_NUMBER];      //speed block number
    struct xp_motor motor[LIFTER_MOTOR_NUMBER];  //lifer motor
    aos_mutex_t lock;
};

#define LIFTER_ORIGIN_ANGLE_OFFSET_KEY  "lifter.origin"
#define LIFTER_MOVE_HEIGHT_MAX          "lifter.top_height"
#define LIFTER_MOVE_HEIGHT_MIN          "lifter.bottom_height"

#define ANGLE_OF_ROTATION_BY_MOVEING_1MM   (72)    // 升降移动一毫米电机需要转动的角度

struct xp_lifter_config {
    unsigned char bus_type;    // modbus can ...
    unsigned char motor_type;  // pans hcfa ...
    unsigned short max_height; // 最大高度
    unsigned short min_height; // 最小高度，配置为零点
};

static struct xp_lifter lifter = {
    .motor_number = LIFTER_MOTOR_NUMBER
};
static u8 move_quit = 0;

// 封装重试机制从kv值中读取相关配置
static xp_bool local_config_kv_get(const char *key, char *value, int *length)
{
    int retry_count = 0;
    for (retry_count = 0; retry_count < 5; retry_count ++) {
        if (0 == aos_kv_get(key, value, length)) {
            return xp_true;
        }
    }
    if (retry_count >= 5) return xp_false;
}

// 对设置KV值得封装
static xp_bool local_config_kv_set(const char *key, const char *value)
{
    return aos_kv_set(key, value, strlen(value), 1) == 0 ? xp_true : xp_false;
}

// 获取当前升降电机相对于零位的旋转偏移角度
static int xp_lifter_current_angle_offset_get()
{
    int num_of_turns = 0, sub_angle = 0;

    if(lifter.motor[0].ops->encode_get(&lifter.motor[0], &num_of_turns, &sub_angle) != 0){
        println("error:get lifter angle offset fail~");
        return -1;
    }
    lifter.angle_offset.current = num_of_turns * 360 + sub_angle;
    return 0;
}

// 获取举平相关配置的kv值并更新到对应的变量
static void xp_lifter_read_kv()
{
    char value[20] = {0};
    int length = 20;
    // 读取零位转换对应的角度偏移信息
    if (xp_true == local_config_kv_get(LIFTER_ORIGIN_ANGLE_OFFSET_KEY, value, &length)) {
        lifter.angle_offset.origin = atoi(value);
        println("aos_kv_get lifter.origin %s", value);
    } else {
        xp_lifter_current_angle_offset_get();
        lifter.angle_offset.origin = lifter.angle_offset.current;
        println("aos_kv_get lifter.origin failed! use current lifter position as zero!");
    }

    //读取升降上限位最大高度
    memset(value, 0, sizeof(value));
    if (xp_true == local_config_kv_get(LIFTER_MOVE_HEIGHT_MAX, value, &length)) {
        lifter.height_offset.max = atof(value);
        println("aos_kv_get lifter.top_height %s", value);
    } else {
        lifter.height_offset.max = LIFTER_top_hight;
        println("aos_kv_get lifter.top_height failed! use default %f", LIFTER_top_hight);
    }
    
    //读取升降下限位最小高度
    memset(value, 0, sizeof(value));
    if (xp_true == local_config_kv_get(LIFTER_MOVE_HEIGHT_MIN, value, &length)) {
        lifter.height_offset.min = atof(value);
        println("aos_kv_get lifter.bottom_height %s", value);
    } else {
        lifter.height_offset.min = LIFTER_bottom_hight;
        println("aos_kv_get lifter.top_height failed! use default %f", LIFTER_top_hight);
    }
}

// 清除举平0位
void xp_lifter_zero_clear()
{
    aos_kv_del("lifter.origin");
}

/**
 * @brief 举平初始化
 * 
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_lifter_init()
{
    // 获取电机对象
    struct xp_modbus_device *dev = get_modbus_device_by_sid(1);
    xp_hcfa_position_motor_constructor(&lifter.motor[0], dev, "lifter_motor");
    // other motor init
    // 清除警告
    xp_clear_lifter_warn();
    // 读取kv配置值
    xp_lifter_read_kv();
    // 编码器值换算为位置
    xp_lifter_get_location();
    
    xp_lifter_change_speed(LIFTER_line_speed);
    // 创建互斥锁
    if (aos_mutex_new(&lifter.lock) != 0) {
        println("create lifter.lock faild.");
        return -4;
    }
    // 创建升降机构执行线程
    void xp_lifter_move_thread(void *arg);
    if(aos_task_new("lifter_move", xp_lifter_move_thread, NULL, 1024) != 0) {
        perr("create lifter list task fail~");
        return -5;
    }

    lifter.inited = 1;  // 升降标记为已初始化

    return 0;
}

// 清除升降电机报警
int xp_clear_lifter_warn()
{
    // do something
    for (int i = 0; i < lifter.motor_number; i ++) {
        lifter.motor[i].ops->error_clear(&lifter.motor[i]);
    }
    return 0;
}

// 改变升降电机的线速度
int xp_lifter_change_speed(float line_speed)
{
    int ret = -1;
    int motor_speed_rpm = 0;

    lifter.move_speed = line_speed;

    motor_speed_rpm = xp_lifter_get_speed(line_speed);
    ret = lifter.motor[0].ops->speed_set(&lifter.motor[0], motor_speed_rpm);
    if(ret != 0) {
        println("change_speed %d failed! ret %d", motor_speed_rpm, ret);
        return ret;
    }
    return 0;
}

// 停止升降运动
static int xp_lifter_move_stop()
{
    if (lifter.inited == 0) return -1;
    int ret = 0;
    for (int i = 0; i < lifter.motor_number; i ++) {
        ret |= lifter.motor[i].ops->rotation_stop(&lifter.motor[i]);
    }

    return ret;
}

// 启动升降运动
static int xp_lifter_move_start()
{
    if (lifter.inited == 0) return -1;
    int ret = 0;
    for (int i = 0; i < lifter.motor_number; i ++) {
        ret |= lifter.motor[i].ops->rotation_start(&lifter.motor[i]);
    }

    return ret;
}

static int xp_lifter_get_pulse(float dis){
    double pulse=0;

    pulse=dis*LIFTER_part*LIFTER_ratio/LIFTER_pitch;
    return (int)pulse;
}

// 将升降距离转换为电机需要转动的角度
static int xp_lifter_motor_rotation_angle_get(float dis)
{
    int angle = 0;
    // 转动72度为行走1mm
    angle = dis * 1000 * ANGLE_OF_ROTATION_BY_MOVEING_1MM;
    return (int)angle;
}

// 设置升降需要运动到的高度
static int xp_lifter_height_set(float height)
{
    if (lifter.inited == 0) return -1;
    int ret = 0;
    float move_height = height - lifter.height_offset.current;
    int dir = (move_height >= 0) ? 1 : -1;      // 1表示向上移动    -1表示向下移动
    move_height = (move_height >= 0) ? move_height : 0 - move_height;   // 取绝对值
    if (move_height < 0.0008) return 0;      // 小于0.0008视为0, 保证一定的误差下任有0.001的移动精度
    int angle = xp_lifter_motor_rotation_angle_get(move_height);
    int num_of_turn = angle / 360;
    int sub_angle = angle % 360;
    lifter.angle_offset.next = (dir == 1) ? lifter.angle_offset.current + angle : lifter.angle_offset.current - angle;
    for (int i = 0; i < lifter.motor_number; i ++) {
        ret |= lifter.motor[i].ops->position_set(&lifter.motor[i], num_of_turn, sub_angle);
        ret |= lifter.motor[i].ops->direction_set(&lifter.motor[i], (dir >= 0) ? motor_forward : motor_reverser);
    }

    return (ret == 0) ? 0 : -2;
}

// 升降运动指定距离所需的时间估算
static int xp_lifter_move_time_estimate(float height)
{
    float move_height = height - lifter.height_offset.current;
    if (move_height < 0) move_height = 0 - move_height;
    float time = move_height / lifter.move_speed;       // 高度单位是米 速度单位是m/s 即时间是秒
    return (int)(time * 1000 + 1000);   // 转换为微秒，并加上1秒的加减速预留时间
}

// 获取电机报警
static int xp_lifter_motor_warn_get()
{
    // 获取报警代码
    int ret = 0;
    for (int i = 0; i < lifter.motor_number; i ++) {
        lifter.motor[i].ops->error_get(&lifter.motor[i], &lifter.warn_code[i]);
        ret |= lifter.warn_code[i];
    }
    return ret;
}

// 清除升降电机的报警
static int xp_lifter_motor_warn_clear()
{
    for (int i = 0; i < lifter.motor_number; i ++) {
        lifter.motor[i].ops->error_clear(&lifter.motor[i]);
    }
    return 0;
}

// 上限位触发检查
static xp_bool xp_lifter_top_limit_touched()
{
    if (xp_io_read_input_pin(1,LIFTER_io_top) == 0) {
        println("lifter top limit touched.");
        return xp_true;
    }
    return xp_false;
}

// 下限位触发限位检查
static xp_bool xp_lifter_bottom_limit_touched()
{
    if (xp_io_read_input_pin(1,LIFTER_io_bottom) == 0) {
        println("lifter bottom limit touched.");
        return xp_true;
    }
    return xp_false;
}

// 升降运动支持重试接口
int xp_lifter_move_support_retry(float dis, u8 speed_block, u8 retry_cnt)
{
    int ret = -1;
    u8 retry_num = 0;

    retry_num = retry_cnt;
    ret = xp_lifter_move(dis, speed_block);
    while(retry_num--){
        if(ret != 0){
            aos_msleep(200);
            println("lifter move failed, retry num %d", retry_num);
            ret = xp_lifter_move(dis, speed_block);
        } else {
            return ret;
        }
    }
    if(ret != 0) {
        println("lifter move failed, retry cnt %d", retry_cnt);
    }
    return ret;
}

/**
 * @brief 升降运动的主要执行函数
 * 
 * @param dis 需要升降的距离
 * @param speed_block 速度块，在新电机中为用到这个参数，这里为了兼容老应用程序而保留
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_lifter_move(float dis, u8 speed_block)
{
    // 判断是否初始化
    if (lifter.inited != 1) return -1;
    if (aos_mutex_lock(&lifter.lock, 100000) != 0) {
        return 1;
    }
    lifter.run = 1;
    // 限位保护
    if (dis < lifter.height_offset.min){
        println("xp_lifter_move: dis %.3f bottom_hight %.3f", dis, lifter.height_offset.min);
        dis = lifter.height_offset.min;
    } else if (dis > lifter.height_offset.max){
        dis = lifter.height_offset.max;
    }

    xp_lifter_get_location();
    if (dis - lifter.height_offset.current < 0.0008 && dis - lifter.height_offset.current > -0.0008) return 0;
    lifter.height_offset.next = dis;
    // 硬限位检查与保护
    if (dis > lifter.height_offset.current && xp_lifter_top_limit_touched()) {
        return -1;
    } else if(dis < lifter.height_offset.current && xp_lifter_bottom_limit_touched()) {
        return -1;
    }

    u32 need_time = xp_lifter_move_time_estimate(dis);
    // 检查警告
    if(xp_lifter_check_warn() != 0 && xp_clear_lifter_warn() != 0) {
        println("The lifter motor reports a warning and a warning that cannot be cleared.");
        return -1;
    }
    // 配置转动距离与速度
    xp_lifter_height_set(dis);
    // 开始转动
    xp_lifter_move_start();
    // 循环判断与等待转动结束
    int start_time = aos_now_ms();
    int abnomal = 0;
    while (1)
    {
        aos_msleep(200);
        // 报警检查
        if (xp_lifter_check_warn() != 0) {
            println("some motor report error or warning.");
            abnomal = -1;
            break;
        }
        // 限位检查
        if (xp_lifter_top_limit_touched() || xp_lifter_bottom_limit_touched()) {
            abnomal = -2;
            break;
        }
        // 超时检查
        if (aos_now_ms() - start_time > need_time) {
            println("lifter move timeout.");
            abnomal = -3;
            break;
        }
        // 读取编码器信息判断定位完成
        
        xp_lifter_get_location();
        println("======= %d ======", lifter.angle_offset.next - lifter.angle_offset.current);
        if ( (lifter.angle_offset.current - lifter.angle_offset.next) < 20 && (lifter.angle_offset.current - lifter.angle_offset.next) > -20 ) {
            aos_msleep(500);
            lifter.height_offset.current = dis;
            abnomal = 0;
            break;
        }
    }
    // 停止运行
    lifter.run = 0;
    xp_lifter_move_stop();
    // 更新定位
    if(abnomal != 0) {
        xp_lifter_get_location();
        println("lifter %d move fail ~ .",abnomal);
    }
    // 解锁
    aos_mutex_unlock(&lifter.lock);

    return abnomal == 0 ? 0 : -1;
}

// 升降单次移动
int xp_lifter_single_move(u8 motor,float dis,u8 speed_block){
    int need_time = xp_lifter_move_time_estimate(dis);
    int pin;
    int angle_gap = xp_lifter_motor_rotation_angle_get(dis);
    int num_of_turn = angle_gap / 360, sub_angle = angle_gap % 360;
    if (lifter.motor[0].ops->position_set(&lifter.motor[0], num_of_turn, sub_angle)) return -1;
    if (lifter.motor[0].ops->direction_set(&lifter.motor[0], (dis >= 0) ? motor_forward : motor_reverser)) return -1;
    lifter.motor[0].ops->rotation_start(&lifter.motor[0]);
    aos_msleep(need_time);
    lifter.motor[0].ops->rotation_stop(&lifter.motor[0]);
    return 0;
}

// 获取升降电机编码器的数据
int xp_lifter_get_encode(u8 motor)
{
    int num_of_turns = 0, sub_angle = 0;

    if(lifter.motor[0].ops->encode_get(&lifter.motor[0], &num_of_turns, &sub_angle) != 0){
        println("error:get lifter encode fail~");
        return -1;
    }
    lifter.angle_offset.current = num_of_turns * 360 + sub_angle;
    return 0;
}

// 获取升降当前的高度
float xp_lifter_get_location()
{
    double angle_gap = 0;

    if(xp_lifter_get_encode(0) != 0) return -1;
    angle_gap = lifter.angle_offset.current - lifter.angle_offset.origin;
    lifter.height_offset.current = (float)(angle_gap / (ANGLE_OF_ROTATION_BY_MOVEING_1MM * 1000));   // 转换为m/s
    return lifter.height_offset.current;
}

#define LIFTER_BUF_size    20
static float move_dis[LIFTER_BUF_size];
static int foot_set=0,foot_get=0,move_init=0;
static aos_sem_t move_sem;
static aos_mutex_t set_mux;

float xp_lifter_move_get(void){
    float ret;
    if(move_init==0){
        perr("lifter list task not inited!");
        return 100.0;
    }
    ret=move_dis[foot_get++];
    if(foot_get==LIFTER_BUF_size){
        foot_get=0;
    }
    return ret;
}

// 升降电机的控制线程
void xp_lifter_move_thread(void *arg)
{
    float dis = 100.0;

    if(aos_mutex_new(&set_mux)!=0){
        perr("create set_mux fail~");
        aos_task_exit(-1);
    }
    if(aos_sem_new(&move_sem,0)!=0){
        perr("create move_sem fail~");
        aos_task_exit(-1);
    }
    move_init=1;
    println("lifter list task create success!!");
    while (1){
        aos_sem_wait(&move_sem,AOS_WAIT_FOREVER);
        dis=xp_lifter_move_get();
        if(dis==100.0){
            continue;
        }
        println("lifter move to %0.3f",dis);
        if(xp_lifter_move(dis,1)!=0){
            perr("lifter move action fail~");
            continue;
        }
    }
}

int xp_lifter_get_speed(float line_speed){
    float motor_rpm;
    
    motor_rpm=line_speed*60.0*LIFTER_ratio/LIFTER_pitch;
    return (int)motor_rpm;
}

int xp_lifter_set_top(char *str){
    if (xp_false == local_config_kv_set(LIFTER_MOVE_HEIGHT_MAX, str)) return -1;
    println("lifter.top_hight set as: %s",str);
    return 0;
}

int xp_lifter_check_warn(void){
    u8 count=LIFTER_motor;
    for(u8 i=0;i<count;i++){
       /* if(xp_pans_get_warn_state(&lifter.motor[i])!=pans_off){
            perr("lifter %d motor have a error !!",i);
            if(xp_pans_get_error_code(&lifter.motor)==0){
                perr("485 connect not normal!!!!  need check why~~~~");
                return 0;
            }
            return -1;
            // if(xp_pans_clear_warn(&lifter.motor[i])!=0)return -2;
            // if(xp_pans_en(&lifter.motor[i],pans_on)!=0)return -1;
        }*/
    }
    return 0;
}

int xp_lifter_relative_move(float value){
    return xp_lifter_move(lifter.height_offset.current+value,1);
}

int xp_lifter_stop(void){
    lifter.motor[0].ops->rotation_stop(&lifter.motor[0]);
    move_quit=1;
    return 0;
}

void xp_lifter_move_clear(void){
    
    aos_mutex_lock(&set_mux,AOS_WAIT_FOREVER);
    for(int i=0;i<LIFTER_BUF_size;i++){
        move_dis[i]=100.0;
    }
    foot_get=0;
    foot_set=0;
    krhino_sem_count_set(move_sem.hdl,0);
    xp_lifter_stop();
    aos_mutex_unlock(&set_mux);
}

// 设置0位
int xp_lifter_as_zero(void)
{
    char value[20] = {0};

    if (xp_lifter_get_encode(0) != 0){
		println("xp_lifter_get_encode failed!");
		return -1;
	}
    lifter.angle_offset.origin = lifter.angle_offset.current;
    lifter.height_offset.current = 0;
    sprintf(value, "%d", lifter.angle_offset.origin);
    if(local_config_kv_set(LIFTER_ORIGIN_ANGLE_OFFSET_KEY, value) != 0) return -2;
    println("lifter.origin set as: %s",value);
    return 0;
}

// 设置最低升降高度
int xp_lifter_set_bottom(char *str){
    if (xp_false == local_config_kv_set(LIFTER_MOVE_HEIGHT_MAX, str)) return -1;
    println("lifter.bottom_hight set as: %s",str);
    return 0;
}

int xp_lifter_move_set(float dis){

    if(move_init==0){
        perr("lifter list task not inited!");
        return -1;
    }
    aos_mutex_lock(&set_mux,AOS_WAIT_FOREVER);
    move_dis[foot_set++]=dis;
    if(foot_set==LIFTER_BUF_size){
        foot_set=0;
    }
    aos_sem_signal(&move_sem);
    aos_mutex_unlock(&set_mux);
    return 0;
}

int xp_lifter_debug(char *type,char *fun,char *param){
    float dis = 0;
    float top_hight = 0;
    float bottom_hight = 0;

    if(strcmp(type,"lifter")!=0)return 0;
    if(strcmp(fun,"move")==0){
        dis=(float)atof(param);
        xp_lifter_move(dis,1);
    }
    else if(strcmp(fun,"support_retry")==0){
        dis=(float)atof(param);
        xp_lifter_move_support_retry(dis,1,3);
    }
    else if(strcmp(fun,"stop")==0){
        xp_lifter_stop();
    }
    else if(strcmp(fun,"move0")==0){
        dis=(float)atof(param);
        xp_lifter_single_move(0,dis,1);
    }
    else if(strcmp(fun,"move1")==0){
        dis=(float)atof(param);
        xp_lifter_single_move(1,dis,1);
    }
    else if(strcmp(fun,"encode")==0){
        xp_lifter_get_encode(0);
        xp_lifter_get_encode(1);
    }
    else if(strcmp(fun,"location")==0){
        println("lifter now location: %0.3f",xp_lifter_get_location());
    }
    else if(strcmp(fun,"check_warn")==0){
        println("warn status: %d",xp_pans_get_warn_state(&lifter.motor[0]));
    }
    else if(strcmp(fun,"get_error")==0){
        xp_pans_get_error_code(&lifter.motor[0]);
    }
    else if(strcmp(fun,"get_encode")==0){
        if(strcmp(param,"0")==0){
            xp_lifter_get_encode(0);
        }
        else if(strcmp(param,"1")==0){
            xp_lifter_get_encode(1);
        }
        else if(strcmp(param,"2")==0){
            xp_lifter_get_encode(0);
            xp_lifter_get_encode(1);
        }
    }
    else if(strcmp(fun,"get_position")==0){
        xp_lifter_get_location();
        println("position: %0.3f m",lifter.height_offset.current);
    }
    else if(strcmp(fun,"set_zero")==0){
        xp_lifter_as_zero();
    }
    else if(strcmp(fun,"clear_zero")==0){
        xp_lifter_zero_clear();
    }
    else if(strcmp(fun,"set_move")==0){
        xp_lifter_move_set((float)atof(param));
    }
    else if(strcmp(fun,"set_clear")==0){
        xp_lifter_move_clear();
    }
    else if(strcmp(fun,"set_top")==0){
        xp_lifter_set_top(param);
    }
    else if(strcmp(fun,"set_bottom")==0){
        xp_lifter_set_bottom(param);
    }
    else if(strcmp(fun,"clear_warn")==0)
    {
        xp_clear_lifter_warn();
    }
    else{
        return 0;
    }
    return 1;
}
